function [msg_cycle_finished, alg_finished] = localization_msg_cycle_step(alg_param)
    msg_cycle_finished = true;
    alg_finished = false;
    
    global g1400_finished;
    global g1400_cur_node;
    global g1400_LMap;
    global g1400_LMap_next;
    
    if g1400_finished == 1
        msg_cycle_finished = true;
        alg_finished = true;
        return;
    end
   
    cur_node = g1400_cur_node;
    % Macierz lokalizacji wezlow i ich statusow
    LMap = g1400_LMap;
    % Typ wezla - latarnia
    latern = 2;
    
    % Sprawdzamy czy pozycje wezla nalezy zaktualizowac (status = 1 i nie
    % latarnia)
    if LMap(cur_node, 1) == 1 && LMap(cur_node, 2) ~= latern
        update_node(cur_node);
        localization_draw_point(cur_node, g1400_LMap_next(cur_node, 2));
    end

end

function update_node(cur_node)
    %% Aktualizacja pozycji wezla
    
    % Szukamy punktow odniesienia
    [anchors, success] = find_anchors(cur_node);
    if ~success
        return;
    end
    % Estymujemy pozycje
    if_notify = estimate_position(cur_node, anchors);
    if ~if_notify
        return;
    end
    % Dajemy znac sasiadom ze zaktualizowalismy pozycje
    notify(cur_node);
end

function [anchors, success] = find_anchors(cur_node)
    %% Szukanie kotwic
    global N_Map;
    global g1400_DDMap;
    global g1400_LMap;
    global g1400_epsilon;
    global g1400_gamma;
    
    % Macierz odleglosci
    DDMap = g1400_DDMap;
    % Macierz lokalizacji
    LMap = g1400_LMap;
    
    anchors = zeros(1,3);
    success = false;
    
    % Liczba sasiadow
    L = N_Map(cur_node, 1);
    if L < 3
        return;
    end
    
    min_e = 1000000;
    anchor = 0;
    % Szukamy pierwszego wezla o najmniejszym e
    for i = 2:L+1
        i_node = N_Map(cur_node, i, 1);
        if find(anchors==i_node)
            continue;
        end
        % Nie bierzemy pod uwage wezlow o typie 0 lub o tym samym polozeniu
        % lub stacji bazowej
        if LMap(i_node, 2) == 0 || DDMap(cur_node, i_node) == 0
            continue;
        end
        
        e = LMap(i_node, 5);
        if e < min_e
            anchor = i_node;
            min_e = e;
        end
    end
    
    if anchor == 0
        return;
    else
        anchors(1) = anchor;
    end
    
    min_e = 1000000;
    anchor = 0;
    % Szukamy drugiego wezla o najmniejszym e
    for i = 2:L+1
        i_node = N_Map(cur_node, i, 1);
        if find(anchors==i_node)
            continue;
        end
        % Nie bierzemy pod uwage wezlow o type 0 lub o tym samym polozeniu
        if LMap(i_node, 2) == 0 || DDMap(cur_node, i_node) == 0
            continue;
        end
        
        % kryterium epsilon
        if dist([LMap(anchors(1), 3) LMap(anchors(1), 4)], [LMap(i_node, 3) LMap(i_node, 4)]) < g1400_epsilon
            continue;
        end
        
        e = LMap(i_node, 5);
        if e < min_e
            anchor = i_node;
            min_e = e;
        end
    end
    
    if anchor == 0
        return;
    else
        anchors(2) = anchor;
    end
    
    min_e = 1000000;
    anchor = 0;
    % Szukamy trzeciego wezla o najmniejszym e
    for i = 2:L+1
        i_node = N_Map(cur_node, i, 1);
        if find(anchors==i_node)
            continue;
        end
        % Nie bierzemy pod uwage wezlow o type 0 lub o tym samym polozeniu
        if LMap(i_node, 2) == 0 || DDMap(cur_node, i_node) == 0
            continue;
        end
        
        % kryterium epsilon wzgledem poprzednich kotwic
        %if (dist([L_Map(anchors(1),3) L_Map(anchors(1), 4)], [L_Map(i,3) L_Map(i,4)]) > g1400_epsilon) || (dist([L_Map(anchors(2),3) L_Map(anchors(2), 4)], [L_Map(i,3) L_Map(i,4)]) > g1400_epsilon)
        %    continue;
        %end
        
        % kryterium gamma
        if minAng([LMap(anchors(1),3) LMap(anchors(1),4)], [LMap(anchors(2),3) LMap(anchors(2),4)], [LMap(i_node,3) LMap(i_node,4)]) < g1400_gamma
            continue;
        end
        
        e = LMap(i_node, 5);
        if e < min_e
            anchor = i_node;
            min_e = e;
        end
    end
    
    if anchor == 0
        return;
    else
        anchors(3) = anchor;
    end
    
    success = true;
end

function [notify] = estimate_position(cur_node, anchors)
    %% Obliczanie pozycji na podstawie kotwic
    global g1400_LMap_next;
    global g1400_LMap;
    global g1400_DDMap;
    global S_Map;
    
    DDMap = g1400_DDMap;
    notify = false;
    orig_pos = [S_Map(cur_node,1) S_Map(cur_node,2)];
    
    r = zeros(1,3);
    
    % Odleglosci kotwic od punktu
    for i=1:3
        r(i) = DDMap(cur_node, anchors(i));
    end
    
    % Pobieramy punty przeciecia okregow
    W = get_point(anchors(1), anchors(2), r(1), r(2), cur_node);
    T = get_point(anchors(1), anchors(3), r(1), r(3), cur_node);
    F = get_point(anchors(2), anchors(3), r(2), r(3), cur_node);
    
%     if W(1) == 0 || T(1) == 0 || F(1) == 0
%         draw_wtf(cur_node, W,T,F, anchors, r);
%     end
    
    % Obliczamy pozycje punktu na podstawie W, T, F
    x = (W(1) + T(1) + F(1)) / 3;
    y = (W(2) + T(2) + F(2)) / 3;
    % Wyliczamy blad
    e = get_e([x y], W, T, F);
    
    %if orig_pos(1) ~= x || orig_pos(2) ~= y
    %    orig_pos
    %    cur_pos = [x y]
    %end
    
    % Jezeli nie zmniejszyl sie blad zostawiamy w spokoju
    cur_e = g1400_LMap(cur_node, 5);
    if cur_e <= e
        return;
    end
    
    % Aktualizujemy tablice dla nastepnej iteracji
    n_type = node_type(e);
    g1400_LMap_next(cur_node, 2) = n_type;
    g1400_LMap_next(cur_node, 3) = x;
    g1400_LMap_next(cur_node, 4) = y;
    g1400_LMap_next(cur_node, 5) = e;
    
    % Jezeli obliczona pozycja zmienila sie powiadamiamy sasiadow
    if n_type > 0 && (g1400_LMap(cur_node, 3) ~= x || g1400_LMap(cur_node, 4) ~= y)
        g1400_LMap_next(cur_node, 3) = x;
        g1400_LMap_next(cur_node, 4) = y;
        notify = true;
    end
end

function [point] = get_point(p1, p2, r1, r2, p3)
    %% Szukanie punktu przeciecia okregow
    global g1400_LMap;
    global S_Map;
    
    LMap = g1400_LMap;
    
    x1 = LMap(p1, 3);
    y1 = LMap(p1, 4);
    x2 = LMap(p2, 3);
    y2 = LMap(p2, 4);
    
    % Wspolrzedne trzeciego punktu
    x3 = S_Map(p3, 1);
    y3 = S_Map(p3, 2);

%     d12 = sqrt((x2-x1)^2 + (y2 - y1)^2);
%     
%     beta = atan((y2 - y1)/(x2 - x1));
%     alfa = acos((r1^2 - r2^2 + d12^2)/(2*r1*d12));
% 
%     x = x1 + r1*cos(alfa + beta);
%     y = y1 + r1*sin(alfa + beta);
%     
%     point = [x, y]

    % Jezeli okregi nie przecinaja sie zwiekszamy ich promienie zeby mialy
    % 1 punkt wspolny
    dist = norm([x1 - x2, y1 - y2]);
    if r1 + r2 < dist
        r1 = r1 + (dist - r1 - r2)/2;
        r2 = dist - r1 + 0.01*dist;
    end

    [x, y] = circcirc(x1, y1, r1, x2, y2, r2);
    
    % Sprawdzamy ile jest punktow przeciecia
    if isnan(x(1))
        point = zeros(1,2);
    elseif x(1) == x(2) && y(1) == y(2)
        point = [x(1), y(1)];
    else
        % Wybieramy blizszy punktowi 3
        d1 = norm([x(1) - x3, y(1) - y3]);
        d2 = norm([x(2) - x3, y(2) - y3]);
        if d1 < d2
            point = [x(1), y(1)];
        else
            point = [x(2), y(2)];
        end
    end
    
    point = roundn(point, -3);
    %point = real(point);
end

function notify(cur_node)
    %% Powiadamianie sasiadow o zmianie pozycji
    global g1400_LMap_next;
    global N_Map;
    
    emit_data(cur_node,false);
    L = N_Map(cur_node,1,1);
    for i = 2:L+1
        nn = N_Map(cur_node, i, 1); % id sasiada
        if ~nn
            continue;
        end
        g1400_LMap_next(nn,1) = 1; % do obrobienia w nastepnej iteracji
        recv_data(nn);
%         localization_draw_point(nn, g1400_LMap_next(nn, 2), 1);
    end
end


function [e] = get_e(p, W, T, F)
    %% Obliczanie bledu e
    e = sqrt((p(1)-W(1))^2 + (p(2)-W(2))^2) + sqrt((p(1)-T(1))^2 + (p(2)-T(2))^2) + sqrt((p(1)-F(1))^2 + (p(2) - F(2))^2);
end

function [type] = node_type(e)
    %% Obliczanie typu wezla na podstawie bledu e
    global g1400_prog1;
    global g1400_prog2;
    
    if e > g1400_prog2
        type = 0; % do niczego
    end
    if e < g1400_prog1
        type = 2; % latarnia
    end
    if e >= g1400_prog1 && e <= g1400_prog2
        type = 1; % kotiwca
    end
    if e == 0
        type = -1;
    end
end

function draw_wtf(cur_node, W, T, F, anchors, r)
    global S_Map;

    plot3(S_Map(anchors(1),1), S_Map(anchors(1),2),300,'yellow.');
    localization_draw_circle(r(1), S_Map(anchors(1),1), S_Map(anchors(1),2));
    plot3(S_Map(anchors(2),1), S_Map(anchors(2),2),300,'yellow.');
    localization_draw_circle(r(2), S_Map(anchors(2),1), S_Map(anchors(2),2));
    plot3(S_Map(anchors(3),1), S_Map(anchors(3),2),300,'yellow.');
    localization_draw_circle(r(3), S_Map(anchors(3),1), S_Map(anchors(3),2));
    
    plot3(W(1), W(2),300,'green.');
    X1=[W(1),S_Map(cur_node,1)];
    Y1=[W(2),S_Map(cur_node,2)];
    Z1=[300,S_Map(cur_node,3)];
    line(X1,Y1,Z1,'Color',[0,1,1]);
    plot3(T(1), T(2),300,'green.');
    X1=[T(1),S_Map(cur_node,1)];
    Y1=[T(2),S_Map(cur_node,2)];
    Z1=[300,S_Map(cur_node,3)];
    line(X1,Y1,Z1,'Color',[0,1,1]);
    plot3(F(1), F(2),300,'green.');
    X1=[F(1),S_Map(cur_node,1)];
    Y1=[F(2),S_Map(cur_node,2)];
    Z1=[300,S_Map(cur_node,3)];
    line(X1,Y1,Z1,'Color',[0,1,1]);
end


function minangle = minAng(p1, p2, p3)
    minangle = min([ang(p1, p2, p3) ang(p1, p3, p2) ang(p3, p1, p2)]);
end

function angle = ang(p1, p2, p3)
    ab = dist(p1, p2);
    bc = dist(p2, p3);
    ac = dist(p1, p3);
    
    cosang = (ab*ab+bc*bc-ac*ac)/(2*ab*bc);
    
    angle = acos(cosang);
end

function distance = dist(p1, p2)
    distance = sqrt((p1(1) - p2(1))^2 + (p1(2) - p2(2))^2);
end